#include "rcl_interfaces/msg/set_parameters_result.hpp"
#include "rclcpp/rclcpp.hpp"

using namespace std::placeholders;
using SetParametersResult = rcl_interfaces::msg::SetParametersResult;

class AddOnSetNode : public rclcpp::Node {
public:
  AddOnSetNode(std::string name) : Node(name) {
    RCLCPP_INFO(this->get_logger(), "add_on_set node start...");
    // 参数声明
    declare_parameter("name", "ilimng");
    declare_parameter("age", 0);
    declare_parameter("height", 1.80);
    declare_parameter("camera", "weijing");
    declare_parameter("camera.exposure_time", 100);
    declare_parameter("camera.color", "black");
    declare_parameter("camera.resolution.width", 640);
    declare_parameter("camera.resolution.height", 480);
    declare_parameter("motor.left.speed", 100);
    declare_parameter("motor.right.speed", 100);
    declare_parameter("sensor.temperature", 25);
    // 注册参数设置回调函数
    // 如果回调函数不是很长，建议使用labmda表达式注册，bind比较复杂
    set_param_callback_handle_ = this->add_on_set_parameters_callback(std::bind(
        &AddOnSetNode::on_set_parameters, this, std::placeholders::_1));
  }

  // 参数设置回调函数
  SetParametersResult
  on_set_parameters(const std::vector<rclcpp::Parameter> &parameters) {
    RCLCPP_INFO(this->get_logger(), "on_set_parameters callback...");
    // 遍历参数列表，打印参数名和参数值
    for (auto param : parameters) {
      RCLCPP_INFO(this->get_logger(), "parameter name: %s, value: %s",
                  param.get_name().c_str(), param.value_to_string().c_str());
      if (param.get_name() == "height") {
        RCLCPP_INFO(this->get_logger(), "parameter name: %s, value: %f",
                    param.get_name().c_str(), param.as_double());
      }
    }
    // 返回成功
    auto result = SetParametersResult();
    result.successful = true;
    return result;
  }

private:
  OnSetParametersCallbackHandle::SharedPtr set_param_callback_handle_;
};

int main(int argc, char **argv) {
  // 初始化ros2的C++接口
  rclcpp::init(argc, argv);
  // 建立节点对象
  auto node = std::make_shared<AddOnSetNode>("add_on_set_node");
  // 运行节点，检测退出信号
  rclcpp::spin(node);
  // 销毁节点并关闭C++接口
  rclcpp::shutdown();

  return 0;
}